#include "freertos/FreeRTOS.h"
#include "driver/ledc.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "tb6612.h"

#define _constrain(amt, low, high) ((amt) > (high) ? (high) : ((amt) < (low) ? (low) : (amt)))

#define TB6612_FREQUENCY              (10000)                    // pwm频率：mg310电机控制频率至少需要10khz
#define TB6612_DUTY_RESOLUTION        LEDC_TIMER_12_BIT          // pwm分辨率：频率越高 分辨率越小
#define TB6612_MAX_RESOLUTION         (4095)                     // pwm占空比分辨率：2^LEDC_TIMER_12_BIT - 1
#define TB6612_MIN_RESOLUTION         (0)                        // pwm占空比分辨率：最小0
#define TB6612_SPEED_MODE             LEDC_LOW_SPEED_MODE        // lcdc时钟源的速度模式
#define TB6612_TIMER0_A               LEDC_TIMER_0
#define TB6612_TIMER0_B               LEDC_TIMER_1
#define TB6612_TIMER_CHANNEL_A        LEDC_CHANNEL_0             // pwm通道  ledc通道共两组，分别为8路高速通道和8路低速通道
#define TB6612_TIMER_CHANNEL_B        LEDC_CHANNEL_1             // pwm通道  ledc通道共两组，分别为8路高速通道和8路低速通道
#define TB6612_GPIO_PWM_A             (15)                        // gpio 6
#define TB6612_GPIO_PWM_B             (16)                        // gpio 7

#define TB6612_STBY                   (17)
#define TB6612_AIN1                   (18)
#define TB6612_AIN2                   (8)
#define TB6612_BIN1                   (3)
#define TB6612_BIN2                   (9)


#define GPIO_OUTPUT_PIN_SEL  ((1ULL<<TB6612_STBY) | (1ULL<<TB6612_AIN1) | \
                              (1ULL<<TB6612_AIN2) | (1ULL<<TB6612_BIN1) | (1ULL<<TB6612_BIN2))

/**
 * @description: tb6612 初始化
 * @param 无
 * @return {*}
 */
void tb6612_init(void)
{

    // 初始化STBY 引脚
    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = GPIO_OUTPUT_PIN_SEL;
    io_conf.pull_down_en = 1;
    io_conf.pull_up_en = 0;
    gpio_config(&io_conf);

    gpio_set_level(TB6612_STBY, 1); // 使能驱动器
    gpio_set_level(TB6612_AIN1, 0); // 正(反)转 
    gpio_set_level(TB6612_AIN2, 1);

    ledc_timer_config_t ledc_timer_initstructure = {
        .clk_cfg = LEDC_USE_APB_CLK,
        .duty_resolution = TB6612_DUTY_RESOLUTION,
        .freq_hz = TB6612_FREQUENCY,
        .speed_mode = TB6612_SPEED_MODE,
        .timer_num = TB6612_TIMER0_A,
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer_initstructure));

    ledc_channel_config_t ledc_channel_initstructure = {
        .channel = TB6612_TIMER_CHANNEL_A,
        .duty = 0,
        .gpio_num = TB6612_GPIO_PWM_A,
        .intr_type = LEDC_INTR_DISABLE,
        .speed_mode = TB6612_SPEED_MODE,
        .timer_sel = TB6612_TIMER0_A,
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel_initstructure));

}

/**
 * @description: 设置sg90占空比
 * @param: duty 设置的占空比 范围：0~(2^13) -1
 * @return {*}
 */
void seTduty(int duty)
{
    ESP_LOGI("tb6612","set_duty=%d\n", duty);

    int strain_duty = _constrain(duty, 0, TB6612_MAX_RESOLUTION);
    ESP_LOGI("tb6612","strain_duty=%d\n", strain_duty);

    ESP_ERROR_CHECK(ledc_set_duty(TB6612_SPEED_MODE, TB6612_TIMER_CHANNEL_A, strain_duty));
    ESP_ERROR_CHECK(ledc_update_duty(TB6612_SPEED_MODE, TB6612_TIMER_CHANNEL_A));

    int get_duty = ledc_get_duty(TB6612_SPEED_MODE, TB6612_TIMER_CHANNEL_A);

    ESP_LOGI("tb6612","get_duty=%d\n", get_duty);
}
